#ifndef CLUSTERING_SPEED_TEST_EUCLIDEAN_CLUSTER_H
#define CLUSTERING_SPEED_TEST_EUCLIDEAN_CLUSTER_H

#include <clustering_speed_test/cluster_base.h>

#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/filters/filter.h>
class Euclidean_cluster : public Cluster_base {
 public:
  Euclidean_cluster(ros::NodeHandle &_nh,YAML::Node& _node) : Cluster_base(_nh,_node) {}
  void clustering(const PointCloudPtr _cloud,std::vector<pcl::PointIndices>& _clustering_indices) override {
    TicToc tic;
    std::vector<int> indices_;
    pcl::removeNaNFromPointCloud(*_cloud,*_cloud,indices_);
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    double euclidean_clustering_tolerance = node["euclidean_clustering_tolerance"].as<double>();
    
    pcl::search::KdTree<Point>::Ptr tree(new pcl::search::KdTree<Point>);
    tree->setInputCloud(_cloud);

    ec.setClusterTolerance(euclidean_clustering_tolerance);  // 欧式聚类的聚类距离阈值
    ec.setMinClusterSize(10);     // 聚类的最小点数
    ec.setMaxClusterSize(25000);   // 聚类的最大点数
    ec.setSearchMethod(tree);  // 设置输入点云
    ec.setInputCloud(_cloud);
    ec.extract(_clustering_indices);  // 执行聚类
    double time_used = tic.toc();
    LOG(INFO) << "time_used : " << time_used;
  }
};

#endif 